#ifndef DW_MATHS_QUATERNION_INL
#	define DW_MATHS_QUATERNION_INL

namespace dw {
	namespace maths {

template< typename T > const Quaternion< T > Quaternion< T >::zero(0, 0, 0, 0);

template< typename T > const Quaternion< T > Quaternion< T >::identity(1, 0, 0, 0);

template< typename T >
inline Quaternion< T >::Quaternion()
{
}

template< typename T >
inline Quaternion< T >::Quaternion(const T& s, const Vector< T, 3 >& v) :
	s(s), v(v)
{
}

template< typename T >
inline Quaternion< T >::Quaternion(const T& w, const T& x, const T& y, const T& z) :
	s(w), v(x, y, z)
{
}

template< typename T >
inline Quaternion< T >::Quaternion(const Vector< T, 3 >& axis, const T& angle)
{
	*this = fromAxisAngle(axis, angle);
}

template< typename T >
inline Quaternion< T >::Quaternion(const Vector< T, 3 >& dir, const Vector< T, 3 >& up)
{
	Vector< T, 3 > z = dir;
	Vector< T, 3 > x = Vector< T, 3 >::cross(up, z);
	Vector< T, 3 > y = Vector< T, 3 >::cross(z, x);

	float tr = x.x + y.y + z.z;

	v.x = y.z - z.y;
	v.y = z.x - x.z;
	v.z = x.y - y.x;
	s = tr + T(1);
	this->normalize();
}

template< typename T >
inline Quaternion< T >::Quaternion(const T& yaw, const T& pitch, const T& roll)
{
	*this = fromEuler(yaw, pitch, roll);
}

template< typename T >
inline Quaternion< T >::Quaternion(const Matrix< T, 3, 3 >& m)
{
	*this = fromMatrix(m);
}

template< typename T >
inline Quaternion< T >&							Quaternion< T >::operator+=(const Quaternion< T >& rhs)
{
	this->s += rhs.s;
	this->v += rhs.v;
	return *this;
}

template< typename T >
inline Quaternion< T >&							Quaternion< T >::operator-=(const Quaternion< T >& rhs)
{
	this->s -= rhs.s;
	this->v -= rhs.v;
	return *this;
}

template< typename T >
inline Quaternion< T >&							Quaternion< T >::operator*=(const Quaternion& rhs)
{
	*this = Quaternion< T >(
		lhs.s * rhs._s - Vector< T, 3 >::dot(lhs._v, rhs._v), 
		lhs.s * rhs._v + rhs._s * lhs._v + Vector< T, 3 >::cross(lhs._v, rhs._v));
	return *this;
}

template< typename T >
inline Quaternion< T >&							Quaternion< T >::operator*=(const T& rhs)
{
	this->s *= rhs;
	this->v *= rhs;
	return *this;
}

template< typename T >
inline Quaternion< T >&							Quaternion< T >::operator/=(const T& rhs)
{
	this->s /= rhs;
	this->v /= rhs;
	return *this;
}

template< typename T >
inline Vector< T, 3 >								Quaternion< T >::getAxis() const
{
	T sin_a = sqrt< T >(T(1) - this->s * this->s);
	if (abs< T >(sin_a) < 0.0005)
		sin_a = 1;
	return this->v / sin_a;
}

template< typename T >
inline T Quaternion< T >::getAngle() const
{
	return acos< T >(this->s) * T(2);
}

template< typename T >
inline T Quaternion< T >::getYaw() const
{
	DW_NOT_IMPLEMENTED;
	return 0;
}

template< typename T >
inline T Quaternion< T >::getPitch() const
{
	DW_NOT_IMPLEMENTED;
	return 0;
}

template< typename T >
inline T Quaternion< T >::getRoll() const
{
	DW_NOT_IMPLEMENTED;
	return 0;
}

template< typename T >
inline std::string Quaternion< T >::toString() const
{
	std::stringstream ss;

	ss << "[" << this->w << ", " << this->x << ", " <<  this->y << ", " << this->z << "]";
	return ss.str();
}

template< typename T >
inline Matrix< T, 3, 3 >						Quaternion< T >::toMatrix() const
{
	T x		= this->x * T(2);
	T y		= this->y * T(2);
	T z		= this->z * T(2);
	T wx	= this->w * x;
	T wy	= this->w * y;
	T wz	= this->w * z;
	T xx	= this->x * x;
	T xy	= this->x * y;
	T xz	= this->x * z;
	T yy	= this->y * y;
	T yz	= this->y * z;
	T zz	= this->z * z;

	return Matrix< T, 3, 3 >(
		T(1) - yy - zz,	xy + wz,				xz - wy,
		xy - wz,				T(1) - xx - zz,	yz + wx,
		xz + wy,				yz - wx,				T(1) - xx - yy);
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::fromAxisAngle(const Vector< T, 3 >& axis, const T& angle)
{
	T halfAngle = angle * T(0.5);

	return Quaternion< T >(::cosf(halfAngle), axis * ::sinf(halfAngle));
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::fromEuler(const T& yaw, const T& pitch, const T& roll)
{
	Quaternion< T > qx, qy, qz;

	qx.fromAxisAngle(Vector< T, 3 >(1, 0, 0), yaw);
	qy.fromAxisAngle(Vector< T, 3 >(0, 1, 0), pitch);
	qz.fromAxisAngle(Vector< T, 3 >(0, 0, 1), roll);

	return qx * qy * qz;
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::fromMatrix(const Matrix< T, 3, 3 >& m)
{
	Quaternion< T >	nrv;
	T w4;

	nrv.w	= std::sqrt(static_cast< T >(1) + m.right.x + m.up.y + m.at.z) / static_cast< T >(2);
	w4		= nrv.w * static_cast< T >(4);
	nrv.x	= (m.up.z - m.at.y) / w4 ;
	nrv.y	= (m.at.x - m.right.z) / w4 ;
	nrv.z	= (m.right.y - m.up.x) / w4 ;
	nrv.normalize();

	return nrv;

	/*Quaternion< T >	q;
	T trace = Matrix< T, 3, 3 >::trace(m);
	T s;

	if (trace > std::numeric_limits< T >::epsilon())
	{
		s = sqrt(trace + 1) * 2;
		q.w = static_cast< T >(0.25) * s;
		q.x = (m(2, 1) - m(1, 2)) / s;
		q.y = (m(0, 2) - m(2, 0)) / s;
		q.z = (m(1, 0) - m(0, 1)) / s;
	}
	else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2))
	{
		s = sqrt(1 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
		q.w = (m(2, 1) - m(1, 2)) / s;
		q.x = static_cast< T >(0.25) * s;
		q.y = (m(1, 0) + m(0, 1)) / s;
		q.z = (m(0, 2) + m(2, 0)) / s;
	}
	else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2))
	{
		s = sqrt(1 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
		q.w = (m(0, 2) - m(2, 0)) / s;
		q.x = (m(1, 0) + m(0, 1)) / s;
		q.y = static_cast< T >(0.25) * s;
		q.z = (m(2, 1) + m(1, 2)) / s;
	}
	else
	{
		s = sqrt(1 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
		q.w = (m(1, 0) - m(0, 1)) / s;
		q.x = (m(0, 2) + m(2, 0)) / s;
		q.y = (m(2, 1) + m(1, 2)) / s;
		q.z = static_cast< T >(0.25) * s;
	}
	return q.normalize();*/
}

template< typename T >
inline T Quaternion< T >::norm() const
{
	return dot(*this, *this);
}

template< typename T >
inline Quaternion< T >&							Quaternion< T >::normalize()
{
	return *this /= sqrtf(norm());
}

template< typename T >
inline T Quaternion< T >::dot(const Quaternion< T >& q0, const Quaternion< T >& q1)
{
	return (q0.s * q1.s + Vector< T, 3 >::dot(q0.v, q1.v));
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::conjugate(const Quaternion< T >& q)
{
	return Quaternion< T >(q.s, -q.v);
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::inverse(const Quaternion< T >& q)
{
	return conjugate(q) /= q.norm();
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::unit(const Quaternion< T >& q)
{
	return Quaternion< T >(q).normalize();
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::rotation(const Vector< T, 3 >& axis, const T& angle)
{
	return Quaternion< T >::fromAxisAngle(axis, angle);
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::rotation(const T& yaw, const T& pitch, const T& roll)
{
	return Quaternion< T >::fromEuler(yaw, pitch, roll);
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::rotation(const Matrix< T, 3, 3 >& m)
{
	return Quaternion< T >::fromMatrix(m);
}

template< typename T >
inline Quaternion< T >							Quaternion< T >::rotation(const Vector< T, 3 >& from, const Vector< T, 3 >& to)
{
	Vector< T, 3 > v0 = from;
	Vector< T, 3 > v1 = to;

	v0.normalize();
	v1.normalize();
	Vector< T, 3 >	axis			= Vector< T, 3 >::cross(v0, v1);
	T cosAngle	= Vector< T, 3 >::dot(v0, v1);

	if (cosAngle >= 1)
		return Quaternion< T >::identity;

	T s			= std::sqrt((1 + cosAngle) * 2);
	T invs	= 1 / s;

	return Quaternion< T >(s * static_cast< T >(0.5), axis * invs);
}

template< typename T >
inline Quaternion< T >					Quaternion< T >::lerp(const Quaternion< T >& q0, const Quaternion< T >& q1, const T& t)
{
	return maths::lerp(q0, q1, t);
}

template< typename T >
inline Quaternion< T >					Quaternion< T >::slerp(const Quaternion< T >& q0, const Quaternion< T >& q1, const T& t)
{
	Quaternion< T > qa = q0;
	Quaternion< T > qb = q1;
	T ka, kb;

	T cosOmega = dot(q0, q1);

	if (cosOmega < T(0))
	{	
		qa = -qa;
		cosOmega = -cosOmega;
	}
	if (cosOmega > 0.9999f)
	{
		ka = 1.0f - t;
		kb = t;
	}
	else 
	{ 
		T omega				= acos(cosOmega);
		T invSinOmega = 1 / sin(omega);
		ka = sin(omega * (1.0f - t)) * invSinOmega;
		kb = sin(omega * t) * invSinOmega;
	}
	return Quaternion< T >(qa * ka + qb * kb);
}

template< typename T >
inline Quaternion< T >					Quaternion< T >::squad(const Quaternion< T >& q0, const Quaternion< T >& q1, const Quaternion< T >& q2, const Quaternion< T >& q3, const T& t)
{
	DW_NOT_IMPLEMENTED;

	return Quaternion< T >();
}

template< typename T >
inline Quaternion< T >					operator-(const Quaternion< T >& rhs)
{
	return Quaternion< T >(-rhs.w, -rhs.x, -rhs.y, -rhs.z);
}

template< typename T >
inline Quaternion< T >					operator~(const Quaternion< T >& rhs)
{
	return Quaternion< T >::conjugate(rhs);
}

template< typename T >
inline Quaternion< T >					operator+(const Quaternion< T >& lhs, const Quaternion< T >& rhs)
{
	Quaternion< T > nrv(lhs);
	nrv += rhs;
	return nrv;
}

template< typename T >
inline Quaternion< T >					operator-(const Quaternion< T >& lhs, const Quaternion< T >& rhs)
{
	Quaternion< T > nrv(lhs);
	nrv -= rhs;
	return nrv;
}

template< typename T >
inline Quaternion< T >					operator*(const Quaternion< T >& lhs, const Quaternion< T >& rhs)
{
	return Quaternion< T >(
		lhs.s * rhs.s - Vector< T, 3 >::dot(lhs.v, rhs.v), 
		lhs.s * rhs.v + rhs.s * lhs.v + Vector< T, 3 >::cross(lhs.v, rhs.v));
}

template< typename T >
inline Quaternion< T >					operator*(const Quaternion< T >& lhs, const T& rhs)
{
	Quaternion< T > nrv(lhs);
	nrv *= rhs;
	return nrv;
}

template< typename T >
inline Quaternion< T >					operator*(const T& lhs, const Quaternion< T >& rhs)
{
	Quaternion< T > nrv(rhs);
	nrv *= lhs;
	return nrv;
}

template< typename T >
inline Quaternion< T >					operator/(const Quaternion< T >& lhs, const T& rhs)
{
	Quaternion< T > nrv(lhs);
	nrv /= rhs;
	return nrv;
}

template< typename T >
inline Vector< T, 3 >						operator*(const Quaternion< T >& q, const Vector< T, 3 >& v)
{
	Quaternion< T > qv = Quaternion< T >(0, v);
	qv = q * qv * Quaternion< T >::inverse(q);
	return qv.v;
}

	} // namespace maths
} // namespace dw

#endif // !DW_MATHS_QUATERNION_INL
